#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include "render/Objects/Renderer3DObject.h"
#include "render/Engine/DrawingEngine.h"
#include "render/render.h"

using namespace pwp;

float ElapsedTime(
	const std::chrono::high_resolution_clock::time_point& begin_time) {
	auto end_time{ std::chrono::high_resolution_clock::now() };
	return float(std::chrono::duration_cast<std::chrono::microseconds>(end_time -
		begin_time)
		.count());
}

int test_pwp_render() {
#ifdef WIN32
	std::string model_path = "D:/dataset/RBOT_dataset/ape/ape.obj";
#else
	std::string model_path = "/home/hh/dataset/rbot/ape/ape.obj";
#endif
	auto scale = 0.001f;
	Eigen::Matrix4f mcm;
	mcm <<
		0.913179, -0.397482, 0.0905337, -0.00461621,
		-0.0362565, -0.300398, -0.953166, -0.00975064,
		0.406031, 0.867085, -0.288712, 9.9f, 
		0, 0, 0, 1;

	Renderer3DObject renderObject(model_path, 0, scale);

	int width = 640, height = 480;
	float fl = 502.299;
	float cx = (width - 1) / 2.f;
	float cy = (height - 1) / 2.f;
	float z_min = 0.02f, z_max = 10.f;

	auto CalcProjectMatrix = [&]() {
		Eigen::Matrix4f pm;
		pm <<
			2.0f * fl / float(width), 0.0f, 
			2.0f * (cx + 0.5f) / float(width) - 1.0f, 0.0f,
			0.0f, 2.0f * fl / float(height), 
			2.0f * (cx + 0.5f) / float(height) - 1.0f, 0.0f,
			0.0f, 0.0f, (z_max + z_min) / (z_max - z_min), 
			-2.0f * z_max * z_min / (z_max - z_min), 0.0f, 0.0f, 1.0f, 0.0f;

		return pm;
	};

	auto pm = CalcProjectMatrix();
	Eigen::Vector4f pt(0, 0, 0, 1);

	//std::cout << pm << std::endl;
	//std::cout << mcm << std::endl;
	//std::cout << pm*mcm << std::endl;
	{
		Eigen::Vector4f ptc = pm * mcm * pt;
		Eigen::Vector3f pti0(ptc.x() / ptc.w(), ptc.y() / ptc.w(), ptc.z() / ptc.w());
		
		pti0[0] = width * (pti0.x() + 1.f) / 2.f;
		pti0[1] = height * (pti0.y() + 1.f) / 2.f;

		std::cout << pti0 << std::endl;
	}

	{
		Eigen::Matrix3f km;
		km <<
			fl, 0, cx,
			0, fl, cy,
			0, 0, 1;

		Eigen::Vector4f ptc = mcm * pt;
		Eigen::Vector3f pt(ptc.x() / ptc.z(), ptc.y() / ptc.z(), 1.f);
		Eigen::Vector3f pti1 = km * pt;
		std::cout << pti1 << std::endl;
	}

	return 0;
}

int test_render_interface() {
	TinyRender render;
	int width = render.width();
	int height = render.height();

#ifdef WIN32
	std::string model_path = "D:/dataset/RBOT_dataset/ape/ape.obj";
#else
	std::string model_path = "/home/hh/dataset/rbot/ape/ape.obj";
#endif
	auto scale = 0.001f;
	float pa[] = {
		0.913179, -0.397482, 0.0905337, -0.00461621,
		-0.0362565, -0.300398, -0.953166, -0.00975064,
		0.406031, 0.867085, -0.288712, 0.24407
	};

	Renderer3DObject renderObject(model_path, 0, scale);

	cv::Mat mask, depth;
	for (int i = 0; i < 20; ++i) {
		auto begin_time = std::chrono::high_resolution_clock::now();
		render.Render(renderObject, pa, &mask, &depth);
		std::cout << "elapsed time = " << 1e-3 * ElapsedTime(begin_time) << std::endl;
	}

	cv::imshow("fill", mask);
	cv::waitKey(0);
	return 0;
}

#if 0
int test_depth() {
	TinyRender render;
	int width = render.width();
	int height = render.height();

	//std::string model_path = "D:/data/RBOT_dataset/ape/ape-s.obj";
	//Renderer3DObject renderObject(model_path, 1, 0, 0.001f);

	//std::string model_path = "D:/data/shiqian/shiqian1/untitled-render.obj";
	//Renderer3DObject renderObject(model_path, 1, 0);

	std::string model_path = "D:/data/benben.simple.unify.g2.obj";
	Renderer3DObject renderObject(model_path, 1, 0, 0.0001f);

	Eigen::Matrix4f mat;
	mat <<
		0.997056, -0.04307, 0.0634383, 0.019051,
		0.043157, 0.999068, 0, 0.00408901,
		-0.0633792, 0.0027378, 0.997986, 0.7,
		0, 0, 0, 1;

	cv::Mat mask, depth;
	Eigen::Transform<float, 3, Eigen::Affine> Tco(mat);
	std::cout << mat << std::endl;

	for (int r = 0; r < 4; r++)
		for (int c = 0; c < 4; c++) {
			std::cout << mat.data()[c + r * 4] << std::endl;
		}

	auto begin_time = std::chrono::high_resolution_clock::now();
	render.Render(renderObject, Tco, &mask, &depth);
	std::cout << "elapsed time = " << 1e-3 * ElapsedTime(begin_time) << std::endl;
	cv::imshow("fill", mask);
	cv::waitKey(0);


	geometry out_body;

	for (int y = 0; y < mask.rows; ++y)
		for (int x = 0; x < mask.cols; ++x) {
			Eigen::Vector3f ptb;
			if (render.CalcBodyPoint(x, y, Tco.inverse(), &ptb)) {
				float3 pt{ ptb(0), ptb(1), ptb(2) };
				out_body.vertices.push_back(pt);
			}
		}

	//WritePly("d:/render_3d.ply", out_body);

	return 0;
}
#endif

#include "soft_render.h"
#include "body.h"
int test_soft_render() {
	int image_width = 640;
	int image_height = 512;
	Intrinsics intrinsics = { 650.048f, 647.183f, 324.328f, 257.323f, image_width, image_height };

#ifdef WIN32
	std::string mesh_path = "D:/dataset/RBOT_dataset/ape/ape.obj";
#else
	std::string mesh_path = "/home/hh/dataset/rbot/ape/ape.obj";
#endif
	std::unique_ptr<Body> body(BodyBuilder()
		.mesh_path(mesh_path, 0.008f)
		.Build());

	SoftRender render(intrinsics);
	render.AddBody(body.get());

	for (int i = 0; i < 20; ++i) {
		auto begin_time = std::chrono::high_resolution_clock::now();
		render.RenderSilhouette();
		std::cout << "elapsed time = " << 1e-3 * ElapsedTime(begin_time) << std::endl;
	}

	render.FetchSilhouetteImage();
	auto disp = render.silhouette_image();
	cv::imshow("disp", disp);

	render.FetchDepthImage();
	auto depth = render.depth_image();
	cv::imshow("depth", depth);

	cv::waitKey();
	return 0;
}

int main(int argc, char* argv[]) {
	//return test_depth();
	//return test_render_interface();
	return test_soft_render();
}